/*
    Temperature output: rear-most sensor to front-most
                        for the beagleboard electronics tube
                pins:   A0, A1, A2, A3
*/

#include <Servo.h>

#define BAUD_RATE 57600
#define NUM_TEMPS 4

Servo servos[5];
int servoPins[] { 2, 4, 7, 12, 13 };
float temps[NUM_TEMPS];
int bbVoltPin = 4;
int pumpAmpPin = 5;
int pumpPin = 51;
unsigned long lastMeasureMs;

void setup() {
  Serial.begin(BAUD_RATE);
  Serial3.begin(9600);

  delay(1000);

  Serial.println("motors");

  delay(3000);
  
  initOutputs();
  
  Serial.println("initializing");
  
  delay(1000);
  
  // stop all motors initially
  setMotorSpeed(0, 0);
  setMotorSpeed(1, 0);
  setMotorSpeed(2, 0);
  setMotorSpeed(3, 0);
  setMotorSpeed(4, 0);
  setMotorSpeed(5, 0);
  
  // stop pump
  digitalWrite(pumpPin, LOW);  

  Serial.println("ready");
  
  lastMeasureMs = millis();
}

int state = 0;
int devIndex;

void loop() {
  if (Serial.available() > 0) {
    if (state == 0) {
      devIndex = Serial.read();
      state = 1;
      
      //Serial.print("devIndex: ");
      //Serial.println(devIndex);
    } else {
      int amt = Serial.read();
      
      if (devIndex >= 0 && devIndex <= 5) {
        amt = (amt - 100) * 1.27;
    
        //Serial.print("sp: ");
        //Serial.println(sp);

        if (amt > 127)
          amt = 127;
        else if (amt < -127)
          amt = -127;
    
        setMotorSpeed(devIndex, amt);
      } else if (devIndex == 6) {
        if (amt > 100) {
          //Serial.println("pump on");
          digitalWrite(pumpPin, HIGH);
        } else {
          //Serial.println("pump off");
          digitalWrite(pumpPin, LOW);
        }
      } else if (devIndex < 12) {
        amt = amt * 0.9;
        
        if (amt > 180)
          amt = 180;
        else if (amt < 0)
          amt = 0;
        
        /*Serial.print("servo: ");
        Serial.print(devIndex);
        Serial.print(" to: ");
        Serial.println(amt);*/
        
        servos[devIndex - 7].write(amt);
      }
      
      state = 0;
    }
  } else {
    tryReadSensors();
  }
}

void initOutputs() {
  Serial.println("setting baud rate");
  Serial3.write(170);
  
  pinMode(pumpPin, OUTPUT);
  
  for (int i = 0; i < 5; i++) {
    pinMode(servoPins[i], OUTPUT);
    servos[i].attach(servoPins[i]);//, 0, 10000);
  }
}

void tryReadSensors() {
  unsigned long now = millis();
  
  if ((now - lastMeasureMs) > 1000) {
    for (int i = 0; i < NUM_TEMPS; i++)
      temps[i] = calcTemp(analogRead(i));
    
    float bbVoltage = calcVolts(analogRead(bbVoltPin));
    float pumpAmps = calcAmps(analogRead(pumpAmpPin));
    
    Serial.print("s");
    
    Serial.print(bbVoltage);
    Serial.print(";");
    Serial.print(pumpAmps);
    Serial.print(";");
    
    for (int i = 0; i < NUM_TEMPS; i++) {
      Serial.print(temps[i]);
      Serial.print(";");
    }
    
    Serial.println();
    
    lastMeasureMs = now;
  }
}

float convAnalogToMilliVolts(int analog) {
  return analog * 5000.0 / 1023.0;
}

// analog input from BB volt meter
float calcVolts(int analog) {
  //return convAnalogToMilliVolts(analog); // / 1000.0 * 13.957 - 36.812;
  return convAnalogToMilliVolts(analog) / 1000.0 * 13.396 - 36.77;
}

// analog input from pump ammeter
float calcAmps(int analog) {
  return convAnalogToMilliVolts(analog);// / 185.0;
}

// analog input from temperature sensor
float calcTemp(int analog) {
  return (convAnalogToMilliVolts(analog) * 33.0 / 380.0) - 48.68;
}

// motorIndex can be [0, 5]
// motorSpeed can be [-127, 127]
void setMotorSpeed(int motorIndex, int motorSpeed) {
  int motorAddr = (motorIndex / 2) + 128;
  int motorCmd = (motorIndex % 2) * 4;
  
  if (motorSpeed < 0) {
    motorCmd = motorCmd + 1;
    motorSpeed = -motorSpeed;
  }

  /*Serial.print("s: ");
  Serial.print(motorIndex);
  Serial.print(" ");
  Serial.print(motorAddr);
  Serial.print(" ");
  Serial.print(motorCmd);
  Serial.print(" ");
  Serial.println(motorSpeed);*/
  
  Serial3.write(motorAddr);
  Serial3.write(motorCmd);
  Serial3.write(motorSpeed);
  Serial3.write((motorAddr + motorCmd + motorSpeed) & 127);
}

